﻿// Copyright (c) Microsoft Corporation. All rights reserved.
// Licensed under the MIT License. See LICENSE in the project root for license information.

using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using HoloToolkit.Unity;
using System;

public class LineDrawer : MonoBehaviour
{
    // Consts
    public const float DefaultLineWidth = 0.001f;
    public const float DefaultBasisLength = 0.2f;

    // Structs
    public class Line
    {
        // Functions
        public Line()
        {
        }
        public Line(Vector3 _p0, Vector3 _p1, Color _c0, Color _c1, float _lineWidth = DefaultLineWidth)
        {
            p0 = _p0;
            p1 = _p1;
            c0 = _c0;
            c1 = _c1;
            lineWidth = _lineWidth;
            isValid = true;
        }
        public bool Set_IfDifferent(Vector3 _p0, Vector3 _p1, Color _c0, Color _c1, float _lineWidth)
        {
            isValid = true;
            if ((p0 != _p0) || (p1 != _p1) || (c0 != _c0) || (c1 != _c1) || (lineWidth != _lineWidth))
            {
                p0 = _p0;
                p1 = _p1;
                c0 = _c0;
                c1 = _c1;
                lineWidth = _lineWidth;
                return true;
            }
            return false;
        }

        // Data
        public Vector3 p0;
        public Vector3 p1;
        public Color c0;
        public Color c1;
        public float lineWidth;
        public bool isValid;
    }
    public class LineData
    {
        public int LineIndex;
        public List<Line> Lines = new List<Line>();
        public MeshRenderer Renderer;
        public MeshFilter Filter;
    }
    public class AnimationCurve3
    {
        public void AddKey(float time, Vector3 pos)
        {
            CurveX.AddKey(time, pos.x);
            CurveY.AddKey(time, pos.y);
            CurveZ.AddKey(time, pos.z);
        }
        public Vector3 Evaluate(float time)
        {
            return new Vector3(CurveX.Evaluate(time), CurveY.Evaluate(time), CurveZ.Evaluate(time));
        }

        public AnimationCurve CurveX = new AnimationCurve();
        public AnimationCurve CurveY = new AnimationCurve();
        public AnimationCurve CurveZ = new AnimationCurve();
    }
    public class AnimatedBox
    {
        public const float InitialPositionForwardMaxDistance = 2.0f;
        public const float AnimationTime = 2.5f;
        public const float DelayPerItem = 0.35f;

        public AnimatedBox(
            float timeDelay,
            Vector3 center,
            Quaternion rotation,
            Color color,
            Vector3 halfSize,
            float lineWidth = DefaultLineWidth * 3.0f)
        {
            TimeDelay = timeDelay;
            Center = center;
            Rotation = rotation;
            Color = color;
            HalfSize = halfSize;
            LineWidth = lineWidth;

            // If no time delay, go ahead and lock the animation now
            if (TimeDelay <= 0.0f)
            {
                SetupAnimation();
            }
        }

        public bool Update(float deltaTime)
        {
            Time += deltaTime;

            // Delay animation setup until after the time delay
            if (!IsAnimationSetup && 
                (Time >= TimeDelay))
            {
                SetupAnimation();
            }

            return (Time >= TimeDelay);
        }

        private void SetupAnimation()
        {
            if (!SpatialUnderstanding.Instance.AllowSpatialUnderstanding)
            {
                return;
            }

            // Calc the forward distance for the animation start point
            Vector3 rayPos = Camera.main.transform.position;
            Vector3 rayVec = Camera.main.transform.forward * InitialPositionForwardMaxDistance;
            IntPtr raycastResultPtr = HoloToolkit.Unity.SpatialUnderstanding.Instance.UnderstandingDLL.GetStaticRaycastResultPtr();
            HoloToolkit.Unity.SpatialUnderstandingDll.Imports.PlayspaceRaycast(
                rayPos.x, rayPos.y, rayPos.z, rayVec.x, rayVec.y, rayVec.z,
                raycastResultPtr);
            SpatialUnderstandingDll.Imports.RaycastResult rayCastResult = HoloToolkit.Unity.SpatialUnderstanding.Instance.UnderstandingDLL.GetStaticRaycastResult();
            Vector3 animOrigin = (rayCastResult.SurfaceType != HoloToolkit.Unity.SpatialUnderstandingDll.Imports.RaycastResult.SurfaceTypes.Invalid) ? 
                rayPos + rayVec.normalized * Mathf.Max((rayCastResult.IntersectPoint - rayPos).magnitude - 0.3f, 0.0f) : 
                rayPos + rayVec * InitialPositionForwardMaxDistance;

            // Create the animation (starting it on the ground in front of the camera
            SpatialUnderstandingDll.Imports.QueryPlayspaceAlignment(SpatialUnderstanding.Instance.UnderstandingDLL.GetStaticPlayspaceAlignmentPtr());
            SpatialUnderstandingDll.Imports.PlayspaceAlignment alignment = SpatialUnderstanding.Instance.UnderstandingDLL.GetStaticPlayspaceAlignment();
            AnimPosition.AddKey(TimeDelay + 0.0f, new Vector3(animOrigin.x, alignment.FloorYValue, animOrigin.z));
            AnimPosition.AddKey(TimeDelay + AnimationTime * 0.5f, new Vector3(animOrigin.x, alignment.FloorYValue + 1.25f, animOrigin.z));
            AnimPosition.AddKey(TimeDelay + AnimationTime * 0.6f, new Vector3(animOrigin.x, alignment.FloorYValue + 1.0f, animOrigin.z));
            AnimPosition.AddKey(TimeDelay + AnimationTime * 0.95f, Center);
            AnimPosition.AddKey(TimeDelay + AnimationTime * 1.0f, Center);

            AnimScale.AddKey(TimeDelay + 0.0f, 0.0f);
            AnimScale.AddKey(TimeDelay + AnimationTime * 0.5f, 0.5f);
            AnimScale.AddKey(TimeDelay + AnimationTime * 0.8f, 1.0f);
            AnimScale.AddKey(TimeDelay + AnimationTime * 1.0f, 1.0f);

            AnimRotation.AddKey(TimeDelay + 0.0f, -1.5f);
            AnimRotation.AddKey(TimeDelay + AnimationTime * 0.2f, -0.5f);
            AnimRotation.AddKey(TimeDelay + AnimationTime * 0.9f, 0.0f);
            AnimRotation.AddKey(TimeDelay + AnimationTime * 1.0f, 0.0f);

            IsAnimationSetup = true;
        }
        public bool IsAnimationComplete { get { return IsAnimationSetup && (Time >= (AnimatedBox.AnimationTime + TimeDelay)); } }

        public Vector3 Center;
        public Quaternion Rotation;
        public Color Color;
        public Vector3 HalfSize;
        public float LineWidth;

        public bool IsAnimationSetup;
        public float Time;
        public float TimeDelay;
        public AnimationCurve AnimScale = new AnimationCurve();
        public AnimationCurve3 AnimPosition = new AnimationCurve3();
        public AnimationCurve AnimRotation = new AnimationCurve();

    }

    // Config
    public Material MaterialLine;

    // Privates
    private LineData lineData = new LineData();

    // Functions
    protected virtual void OnDestroy()
    {
        // Line renderer
        if (lineData != null)
        {
            if (lineData.Renderer != null)
            {
                Destroy(lineData.Renderer);
            }
            if (lineData.Filter != null)
            {
                Destroy(lineData.Filter);
            }
        }
        lineData = null;
    }

    protected const int PointsOnCircle = 50;
    protected bool Draw_Circle(Vector3 center, Vector3 normal, Color color, float radius = 0.25f, float lineWidth = DefaultLineWidth)
    {
        bool returnValue = false;
        float theta = 0;
        float radPerPoint = (2.0f * Mathf.PI) / (float)PointsOnCircle;
        Quaternion q = Quaternion.FromToRotation(Vector3.up, normal);
        Vector3 start = q * new Vector3(Mathf.Cos(theta) * radius, 0.0f, Mathf.Sin(theta) * radius) + center;

        for (int i = 1; i <= PointsOnCircle; i++)
        {
            theta = (i % PointsOnCircle) * radPerPoint;

            Vector3 end = q * new Vector3(Mathf.Cos(theta) * radius, 0.0f, Mathf.Sin(theta) * radius) + center;
            returnValue |= Draw_Line(start, end, color, color, lineWidth);

            start = end;
        }

        return returnValue;
    }

    protected bool Draw_Circle_Partial(Vector3 center, Vector3 normal, Color color, float radius = 0.25f, float lineWidth = DefaultLineWidth, float circleAngleArc = 360.0f)
    {
        bool returnValue = false;
        float theta = 0;
        float radPerPoint = (circleAngleArc * Mathf.Deg2Rad * 0.5f) / (float)PointsOnCircle;
        Quaternion q = Quaternion.FromToRotation(Vector3.up, normal);
        Vector3 start0 = q * new Vector3(Mathf.Cos(theta) * radius, 0.0f, Mathf.Sin(theta) * radius) + center;
        Vector3 start1 = q * new Vector3(Mathf.Cos(theta) * -radius, 0.0f, Mathf.Sin(theta) * -radius) + center;

        int maxPointCount = (circleAngleArc < 360.0f) ? (PointsOnCircle - 1) : PointsOnCircle;
        for (int i = 1; i <= maxPointCount; i++)
        {
            theta = (i % PointsOnCircle) * radPerPoint;

            Vector3 end0 = q * new Vector3(Mathf.Cos(theta) * radius, 0.0f, Mathf.Sin(theta) * radius) + center;
            Vector3 end1 = q * new Vector3(Mathf.Cos(theta) * -radius, 0.0f, Mathf.Sin(theta) * -radius) + center;
            returnValue |= Draw_Line(start0, end0, color, color, lineWidth);
            returnValue |= Draw_Line(start1, end1, color, color, lineWidth);

            start0 = end0;
            start1 = end1;
        }

        return returnValue;
    }

    protected bool Draw_Cube(Vector3 point, Color color, float halfSize = DefaultLineWidth)
    {
        return Draw_Line(point - Vector3.right * halfSize, point + Vector3.right * halfSize, color, color, halfSize);
    }

    protected bool Draw_AnimatedBox(AnimatedBox box)
    {
        // Update the time
        if (!box.Update(Time.deltaTime))
        {
            return false;
        }
        if (box.IsAnimationComplete)
        {
            // Animation is done, just pass through
            return Draw_Box(box.Center, box.Rotation, box.Color, box.HalfSize, box.LineWidth);
        }

        // Draw it using the current anim state
        return Draw_Box(
            box.AnimPosition.Evaluate(box.Time),
            box.Rotation * Quaternion.AngleAxis(360.0f * box.AnimRotation.Evaluate(box.Time), Vector3.up),
            box.Color,
            box.HalfSize * box.AnimScale.Evaluate(box.Time),
            box.LineWidth);
    }

    protected bool Draw_Box(Vector3 center, Quaternion rotation, Color color, Vector3 halfSize, float lineWidth = DefaultLineWidth)
    {
        bool needsUpdate = false;

        Vector3 basisX = rotation * Vector3.right;
        Vector3 basisY = rotation * Vector3.up;
        Vector3 basisZ = rotation * Vector3.forward;
        Vector3[] pts =
        {
            center + basisX * halfSize.x + basisY * halfSize.y + basisZ * halfSize.z,
            center + basisX * halfSize.x + basisY * halfSize.y - basisZ * halfSize.z,
            center - basisX * halfSize.x + basisY * halfSize.y - basisZ * halfSize.z,
            center - basisX * halfSize.x + basisY * halfSize.y + basisZ * halfSize.z,

            center + basisX * halfSize.x - basisY * halfSize.y + basisZ * halfSize.z,
            center + basisX * halfSize.x - basisY * halfSize.y - basisZ * halfSize.z,
            center - basisX * halfSize.x - basisY * halfSize.y - basisZ * halfSize.z,
            center - basisX * halfSize.x - basisY * halfSize.y + basisZ * halfSize.z
        };

        // Bottom
        needsUpdate |= Draw_Line(pts[0], pts[1], color, color, lineWidth);
        needsUpdate |= Draw_Line(pts[1], pts[2], color, color, lineWidth);
        needsUpdate |= Draw_Line(pts[2], pts[3], color, color, lineWidth);
        needsUpdate |= Draw_Line(pts[3], pts[0], color, color, lineWidth);

        // Top
        needsUpdate |= Draw_Line(pts[4], pts[5], color, color, lineWidth);
        needsUpdate |= Draw_Line(pts[5], pts[6], color, color, lineWidth);
        needsUpdate |= Draw_Line(pts[6], pts[7], color, color, lineWidth);
        needsUpdate |= Draw_Line(pts[7], pts[4], color, color, lineWidth);

        // Vertical lines
        needsUpdate |= Draw_Line(pts[0], pts[4], color, color, lineWidth);
        needsUpdate |= Draw_Line(pts[1], pts[5], color, color, lineWidth);
        needsUpdate |= Draw_Line(pts[2], pts[6], color, color, lineWidth);
        needsUpdate |= Draw_Line(pts[3], pts[7], color, color, lineWidth);

        return needsUpdate;
    }

    protected bool Draw_Line(Vector3 start, Vector3 end, Color colorStart, Color colorEnd, float lineWidth = DefaultLineWidth)
    {
        // Create up a new line (unless it's already created)
        while (lineData.LineIndex >= lineData.Lines.Count)
        {
            lineData.Lines.Add(new Line());
        }

        // Set it
        bool needsUpdate = lineData.Lines[lineData.LineIndex].Set_IfDifferent(transform.InverseTransformPoint(start), transform.InverseTransformPoint(end), colorStart, colorEnd, lineWidth);

        // Inc out count
        ++lineData.LineIndex;

        return needsUpdate;
    }

    protected bool Draw_TransformBasis(Transform transformToDraw, float basisLength = DefaultBasisLength, float lineWidth = DefaultLineWidth * 2.0f)
    {
        // Basis
        bool needsUpdate = false;
        needsUpdate |= Draw_Line(transformToDraw.transform.position, transformToDraw.transform.position + transformToDraw.transform.right * basisLength, Color.red, Color.red, lineWidth);
        needsUpdate |= Draw_Line(transformToDraw.transform.position, transformToDraw.transform.position + transformToDraw.transform.up * basisLength, Color.green, Color.green, lineWidth);
        needsUpdate |= Draw_Line(transformToDraw.transform.position, transformToDraw.transform.position + transformToDraw.transform.forward * basisLength, Color.blue, Color.blue, lineWidth);

        return needsUpdate;
    }

    private void Lines_LineDataToMesh()
    {
        // Alloc them up
        Vector3[] verts = new Vector3[lineData.Lines.Count * 8];
        int[] tris = new int[lineData.Lines.Count * 12 * 3];
        Color[] colors = new Color[verts.Length];

        // Build the data
        for (int i = 0; i < lineData.Lines.Count; ++i)
        {
            // Base index calcs
            int vert = i * 8;
            int v0 = vert;
            int tri = i * 12 * 3;

            // Setup
            Vector3 dirUnit = (lineData.Lines[i].p1 - lineData.Lines[i].p0).normalized;
            Vector3 normX = Vector3.Cross((Mathf.Abs(dirUnit.y) >= 0.99f) ? Vector3.right : Vector3.up, dirUnit).normalized;
            Vector3 normy = Vector3.Cross(normX, dirUnit);

            // Verts
            verts[vert] = lineData.Lines[i].p0 + normX * lineData.Lines[i].lineWidth + normy * lineData.Lines[i].lineWidth; colors[vert] = lineData.Lines[i].c0; ++vert;
            verts[vert] = lineData.Lines[i].p0 - normX * lineData.Lines[i].lineWidth + normy * lineData.Lines[i].lineWidth; colors[vert] = lineData.Lines[i].c0; ++vert;
            verts[vert] = lineData.Lines[i].p0 - normX * lineData.Lines[i].lineWidth - normy * lineData.Lines[i].lineWidth; colors[vert] = lineData.Lines[i].c0; ++vert;
            verts[vert] = lineData.Lines[i].p0 + normX * lineData.Lines[i].lineWidth - normy * lineData.Lines[i].lineWidth; colors[vert] = lineData.Lines[i].c0; ++vert;

            verts[vert] = lineData.Lines[i].p1 + normX * lineData.Lines[i].lineWidth + normy * lineData.Lines[i].lineWidth; colors[vert] = lineData.Lines[i].c1; ++vert;
            verts[vert] = lineData.Lines[i].p1 - normX * lineData.Lines[i].lineWidth + normy * lineData.Lines[i].lineWidth; colors[vert] = lineData.Lines[i].c1; ++vert;
            verts[vert] = lineData.Lines[i].p1 - normX * lineData.Lines[i].lineWidth - normy * lineData.Lines[i].lineWidth; colors[vert] = lineData.Lines[i].c1; ++vert;
            verts[vert] = lineData.Lines[i].p1 + normX * lineData.Lines[i].lineWidth - normy * lineData.Lines[i].lineWidth; colors[vert] = lineData.Lines[i].c1; ++vert;

            // Indices
            tris[tri + 0] = (v0 + 0); tris[tri + 1] = (v0 + 5); tris[tri + 2] = (v0 + 4); tri += 3;
            tris[tri + 0] = (v0 + 1); tris[tri + 1] = (v0 + 5); tris[tri + 2] = (v0 + 0); tri += 3;

            tris[tri + 0] = (v0 + 1); tris[tri + 1] = (v0 + 6); tris[tri + 2] = (v0 + 5); tri += 3;
            tris[tri + 0] = (v0 + 2); tris[tri + 1] = (v0 + 6); tris[tri + 2] = (v0 + 1); tri += 3;

            tris[tri + 0] = (v0 + 2); tris[tri + 1] = (v0 + 7); tris[tri + 2] = (v0 + 6); tri += 3;
            tris[tri + 0] = (v0 + 3); tris[tri + 1] = (v0 + 7); tris[tri + 2] = (v0 + 2); tri += 3;

            tris[tri + 0] = (v0 + 3); tris[tri + 1] = (v0 + 7); tris[tri + 2] = (v0 + 4); tri += 3;
            tris[tri + 0] = (v0 + 3); tris[tri + 1] = (v0 + 4); tris[tri + 2] = (v0 + 0); tri += 3;

            tris[tri + 0] = (v0 + 0); tris[tri + 1] = (v0 + 3); tris[tri + 2] = (v0 + 2); tri += 3;
            tris[tri + 0] = (v0 + 0); tris[tri + 1] = (v0 + 2); tris[tri + 2] = (v0 + 1); tri += 3;

            tris[tri + 0] = (v0 + 5); tris[tri + 1] = (v0 + 6); tris[tri + 2] = (v0 + 7); tri += 3;
            tris[tri + 0] = (v0 + 5); tris[tri + 1] = (v0 + 7); tris[tri + 2] = (v0 + 4); tri += 3;
        }

        // Create up the components
        if (lineData.Filter == null)
        {
            lineData.Filter = gameObject.AddComponent<MeshFilter>();
        }
        if (lineData.Renderer == null)
        {
            lineData.Renderer = gameObject.AddComponent<MeshRenderer>();
            lineData.Renderer.material = MaterialLine;
        }

        // Create or clear the mesh
        Mesh mesh = null;
        if (lineData.Filter.mesh != null)
        {
            mesh = lineData.Filter.mesh;
            mesh.Clear();
        }
        else
        {
            mesh = new Mesh();
            mesh.name = "LineDrawer.Lines_LineDataToMesh";
        }

        // Set them into the mesh
        mesh.vertices = verts;
        mesh.triangles = tris;
        mesh.colors = colors;
        mesh.RecalculateBounds();
        mesh.RecalculateNormals();

        lineData.Filter.mesh = mesh;

        // If no tris, hide it
        lineData.Renderer.enabled = (lineData.Lines.Count == 0) ? false : true;

        // Line index reset
        lineData.LineIndex = 0;
    }

    protected void LineDraw_Begin()
    {
        lineData.LineIndex = 0;
        for (int i = 0; i < lineData.Lines.Count; ++i)
        {
            lineData.Lines[i].isValid = false;
        }
    }

    protected void LineDraw_End(bool needsUpdate)
    {
        if (lineData == null)
        {
            return;
        }

        // Check if we have any not dirty
        int i = 0;
        while (i < lineData.Lines.Count)
        {
            if (!lineData.Lines[i].isValid)
            {
                needsUpdate = true;
                lineData.Lines.RemoveAt(i);
                continue;
            }
            ++i;
        }

        // Do the update (if needed)
        if (needsUpdate)
        {
            Lines_LineDataToMesh();
        }
    }
}
